package pteam;

import java.io.*;
import java.util.*;

import simulator.Actor;
import simulator.Robot;
import simulator.Sensor;
import simulator.BumpSensor;

//todo : add seperate ahead for left and right to improve intelligence of avoid() turning

public class GoldRushActor implements Actor {
  private double rl; //range sensor - left
  private double rr; //range sensor - right
  boolean trouble = false;
  boolean ahead = false;		//says whether the range finders have detected anything within TROUBLE_RANGE of the bot
  boolean aheadl = false; //ahead left and right
  boolean aheadr = false;
  int beaconDirection = 0;
  private int direction = 0;

  private static final double TROUBLE_RANGE = 10; //threshold for range finder
  
  private boolean combing = false;
  private boolean cfwd = true; //combing forward
  private boolean crturn = true; //combing: time to turn right? (if false, left)
  
  private boolean inred;
  private boolean ingreen;
  private boolean inboth;
  
  private int adjustment = 0; //used in avoid() - positive = right, negative = left
  
  private static final int FWD_WAIT = 1; //time to moveForward() normally
  private static final int TROUBLE_WAIT = /*10*/6; //time to moveForward() when avoiding obstacles
  // smaller TROUBLE_WAIT lets the robot get through gaps in obstacles that it would otherwise miss
  private static final int BACK_WAIT = 5; //time to back up in avoid()
  private static final int STEP_WAIT = 10; //time to move - determines spacing of columns of movement when combing
  
  private static final int AVD_ANGLE = 90; //angle used when avoiding obstacles
  private static final int BCN_SEEK_ANGLE = 10; //angle used when seeking beacon
  
  private int acts = 0; //number of times act() has been called
  private int lastavd = 0; //index of act() wherein avoid() was called 
  
  private boolean avdright = true; //avoid by turning right? (false = left)
  private static final int SWITCH_AVDDIR_ACTS = 135; //number acts must increase by w/o avoid() being called for avdright to switch
  //(meaning threshold acts-lastavd must pass
  
  private static final double RANGE_ERROR_THRESH = .1; //error in range sensor readings allowed (used in reached())
  private static final int REACHED_ANGLE = 30; //angle used for turning to scan in reached()
  private static final double WALL_LEN = 30; //minimum length of obstacle for it to be considered a "wall"
  
  
  //states
  //(used for multiple-act jobs)
  private boolean movingfwd = false;
  private int fwait = 0; //time to move fwd
  private int trouble_back = 0; //time to move back if trouble had in moving fwd
  private boolean adjustdue = false;
  
  public GoldRushActor() {
  }  // TODO(kroo): someone implement    !! 
  
  
  public String getMaze() {
    return "Gold";
  }

  public void act(Robot r) {
    System.out.println("a" + acts); acts++;
    if(acts - lastavd >= SWITCH_AVDDIR_ACTS)
      avdright = !avdright;
    if(movingfwd) {
    	System.out.println("fwd...");
    	if(!trouble)
    	  moveForward(r, fwait);
    	else{
    	  movingfwd = false;
    	  if(trouble_back != 0) {
    	    moveBackward(r, trouble_back);
    	    trouble_back = 0;
    	  }
    	}
    }else if(adjustdue) {
    	System.out.println("adjusting");
    	adjust(r);
    	adjustdue = false;
    }else if(beaconSpotted()) {
      System.out.println("beacon spotted");
      turnTowardsBeacon(r);
      System.out.println("done turning towards it");
      charge(r);		// what does this do?
      System.out.println("done charging towards it");
    }else if(!combing && reached(r)) {
      System.out.println("just reached!");
      //adjustAtWall(r); //correct errors in orientation
      turnSouth(r);
      combing = true;
    }else if(combing){
      System.out.println("combing");
      if(reached(r)) {
        cfwd = false;
        //adjustAtWall(r);
      }
      comb(r);
    }else{
      System.out.println("just charging");
      charge(r);
    }
  }
  
  /**
   * move forward until the bot can't any more
   * @param r
   */
  public void charge(Robot r) {
    if(trouble()) {
      //System.out.println("trouble detected in charge! -- avoiding");
      System.out.println("..");
      if(trouble) {
      	System.out.println("-trouble - have to move back");
        moveBackward(r, TROUBLE_WAIT / 2); //if the touch sensor is bumped, move back a bit
        System.out.println("done");
      }
      System.out.println("i have to call avoid()");
      avoid(r);				// turn right
      System.out.println("avoid() called and executed");
      trouble_back = TROUBLE_WAIT / 3;
      moveForward(r, TROUBLE_WAIT);	// go straight
      System.out.println("adjusting");
      adjustdue = true;			// turn left
    }else{
        System.out.println("moving forward");
      	moveForward(r, FWD_WAIT);
        System.out.println("moved");
    }
  }

  private boolean beaconSpotted() {
    //System.out.println("red " + inred + " both " + inboth + " green " + ingreen);
    return inred || inboth || ingreen;
  }

  private boolean trouble() {
    //trouble: touched
    //ahead: problem ahead
    return trouble || ahead;
  }

  /**
   * turn away from the obstacle detected - backs up and turns right
   */
  private void avoid(Robot r) {
    System.out.println("avoid() called");
    lastavd = acts;
    moveBackward(r, BACK_WAIT);
    System.out.println("-moved bwd");
    turnAngle(r, (avdright? 1 : -1) * AVD_ANGLE);
    System.out.println("-turned");
    adjustment += (avdright? 1 : -1) * AVD_ANGLE;
    System.out.println("avoid() done");
  }

  private void comb(Robot r) {
    if(cfwd) {
      charge(r);
    }else{
      if(crturn) { //turns, moves forward 1 step, turns
        turnRight(r);
        step(r);
        turnRight(r);
      }else{
        turnLeft(r);
        step(r);
        turnLeft(r);
      }
      crturn = !crturn;
      cfwd = true;
    }
  }

  public void updateSensors(Sensor[] sensors) {
    //System.out.println("u");
    //System.out.println("updating sensor");
    //System.out.println("Found wall at range "
    //    + sensors[1].getState()[0]);
    rl = sensors[1].getState()[0]; //range sensor - left
    rr = sensors[3].getState()[0]; //range sensor - right
    aheadl = rl < TROUBLE_RANGE; //do the range sensors detect anything w/in TROUBLE_RANGE of the bot?
    aheadr = rr < TROUBLE_RANGE;
    ahead = aheadl || aheadr;
    trouble = sensors[0].getState()[0] == BumpSensor.PRESSED; // touch sensors are to be used as last resort ONLY
    //Simulator.out.println("gr" + ((BeaconSensor)sensors[5]).INGREEN + " r" + ((BeaconSensor)sensors[5]).INRED + " b" + ((BeaconSensor)sensors[5]).INBOTH);
    ingreen = false; inred = false; inboth = false; //remove once beacon sensing works
  }
  
  /**
   * turns the robot a specified angle
   * @param r
   * @param angle the angle to turn (positive = clockwise, negative = counter-)
   */
  public void turnAngle(Robot r, int angle) {
    if(angle > 0) {
      r.turnRight();
      r.waitAngle(angle);
    }else if(angle < 0) { //needed to ignore 0
      r.turnLeft();
      r.waitAngle(-angle);
    }
  }
  
  public void turnLeft(Robot r) {
    turnAngle(r, -90);
    direction += 3;
    direction %= 4;
  }

  public void turnRight(Robot r) {
    turnAngle(r, 90);
    direction += 1;
    direction %= 4;
  }

  /**
   * turn the bot to a certain *absolute* direction
   * @param r the Robot to turn
   * @param dir the enumerated direction to turn to
   */
  public void turn(Robot r, int dir) {
    if(direction == (dir + 1) % 4) {
      turnLeft(r);
    }else{
      while(direction != dir) {
        turnRight(r);
      }
    }
  }
  public void turnNorth(Robot r) {
    turn(r, 0);
  }

  public void turnEast(Robot r) {
    turn(r, 1);
  }

  public void turnWest(Robot r) {
    turn(r, 0);
  }

  public void turnSouth(Robot r) {
    turn(r, 2);
  }
  
  public void moveForward(Robot r, int wait) {
    if(!movingfwd) {
      movingfwd = true;
      fwait = wait;
    }else if(wait == 0 || wait == 1) {
    	r.goForward();
    	r.waitDistance(wait);
    	movingfwd = false;
    }else{
    	fwait = wait - 1;
    	r.goForward();
    	r.waitDistance(1);
    }
  }
  
  public void moveBackward(Robot r, int wait) {
    r.goBackward();
    r.waitDistance(wait);
  }
  
  /**
   * turn back to how the bot was oriented before avoid() was called
   * @param r
   */
  public void adjust(Robot r) {
    turnAngle(r, -adjustment);
    adjustment = 0;
  }
  
  /**
   * @return whether the opposite wall has been reached (currently just sees if both range and touch sensors detect an object)
   */
  public boolean reached(Robot r) {
  //perhaps add code here to turn the bot and see if the change in range finder readings confirm presence of wall
  //(todo: use the inverse secant of range sensor readings and see if they correspond to bot angle)
    /*if(!trouble && !ahead)
      return false;
    turnAngle(r, REACHED_ANGLE);
    double l0 = rl; //left range sensor - reading 0
    double r0 = rr; //right "  " - " "
    turnAngle(r, -REACHED_ANGLE);
    double l1 = rl;
    double r1 = rr;
    turnAngle(r, -REACHED_ANGLE);
    double l2 = rl;
    double r2 = rr;
    turnAngle(r, REACHED_ANGLE);
    
    //magic trig - makes sense since the graph of sec(theta) (polar, obviously) is a vertical line to the right of the origin
    //(and asec(x) = acos(1/x))
    //so we expect acos(1/[reading]) to be vary linearly with the bot's angle, and thus acos(1/[reading])/[angle] to be a constant
    //(note: constants differ for left and right readings)
    //so we just see how long the readings hold this property and that gives us the length of the possible wall
    //of course the sensors lie slightly so RANGE_ERROR_THRESH provides some slack
    //CAUTION: PLEASE DON'T DIVIDE BY 0
    
    int angle = 0; //bot angle
    double lcon = Math.acos(1 / rl) / -45; //left constant
    double rcon = Math.acos(1 / rr) / 45; //right constant
    
    double llen = 0; //left length measured
    double rlen = 0;
    double clen = 0; //center length
    double newcon;
    do{
      turnAngle(r, -1);
      angle -= 1;
      newcon = Math.acos(1 / rl) / (-45 + angle);
    }while(Math.abs(lcon - newcon) < RANGE_ERROR_THRESH);
    llen = rl * (Math.sin(45 - angle) - Math.sin(45));
    turnAngle(r, -angle);
    angle = 0;
    do{
    	turnAngle(r, -1);
    	angle += 1;
    	newcon = Math.acos(1 / rr) / (45 + angle);
    }while(Math.abs(rcon - newcon) < RANGE_ERROR_THRESH);
    rlen = rr * (Math.sin(45 + angle) - Math.sin(45));
    turnAngle(r, -angle);
    angle = 0;
    do{
    	turnAngle(r, 1);
    	angle += 1;
    	newcon = Math.acos(1 / rl) / (-45 + angle);
    }while(Math.abs(lcon - newcon) < RANGE_ERROR_THRESH && angle < 90);
    clen = rl * (Math.sin(angle - 45) + Math.sin(45));
    turnAngle(r, -angle);
    
    return angle >= 90 && (rlen + clen + llen >= WALL_LEN);*/
    //return trouble || ahead;
    return false;
  }
  
  /**
   * reorient the bot at the wall to correct any random error in its angle
   * (turns the bot so it is facing the wall, with both range sensors reading equal values)
   */
  public void adjustAtWall(Robot r) {
    while(rl > rr) { //skipped if the opposite is the problem
      turnAngle(r, -1);
    }
    while(rr > rl) {
      turnAngle(r, 1);
    }
  }
  /**
   * beacon detected - now orient the bot towards it
   */
  public void turnTowardsBeacon(Robot r) {
    if(!inboth) {
      if(inred) {
        turnAngle(r, -BCN_SEEK_ANGLE);
      }else if(ingreen) {
        turnAngle(r, BCN_SEEK_ANGLE);
      }
    }
  }
  
  /**
   * move forward (used between the 2 turns of comb() to determine spacing of movement columns)
   */
  public void step(Robot r) {
    moveForward(r, STEP_WAIT);
  }
}